package com.zhdl.modules.autopick.service.impl;

import com.project.common.util.DelayUtil;
import com.zhdl.modules.autopick.service.CmdHandleService;
import com.zhdl.modules.autopick.service.RobotActionService;
import lombok.extern.slf4j.Slf4j;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.stereotype.Service;

@Slf4j
@Service
public class RobotActionServiceImpl implements RobotActionService {


    public static int pickZ = 1225802;
    @Autowired
    private CmdHandleService cmdHandleService;

    /*
        M010 --> M047 160 --> 191 A0 --> BF
        M110 --> M147 192 --> 223 C0 --> DF
        M210 --> M247 224 --> 255 E0 --> FF
     */

    /**
     * 读取当前状态
     * - ready 待机 run 运动
     */
    @Override
    public void getRobotStatus() {
        log.info("获取机器人当前状态");
    }

    /**
     * 机器人-读取机器人坐标
     */
    @Override
    public void getRobotCoordinate() {
        //读取机器人世界坐标
        cmdHandleService.robotStatusCoordinate();
        log.info("获取机器人当前世界坐标 ");
    }

    /**
     * 机器人-机械臂 初始化
     */
    @Override
    public void robotArmInit(int[] currentLocation) {
        //2. 设置上抬位置
        cmdHandleService.robotSetZLocation(currentLocation);
        DelayUtil.delayedExecution(1000, new Runnable() {
            @Override
            public void run() {
                //3. 设置M12 通知回原点信号
                cmdHandleService.robotM12(162, true);
            }
        });
        log.info("机器人-机械臂 初始化 M12");
    }

    /**
     * 读完销毁
     *
     * @param address
     * @param open
     */
    @Override
    public void writeFalseM(int address, boolean open) {
        cmdHandleService.robotM11(address, open);
        log.info("机器人-销毁地址为:" + address + "的信号,销毁结果为" + !open);
    }

    /**
     * 机器人写等待放料信号 M11
     */
    @Override
    public void writeM11() {
        cmdHandleService.robotM11(161, true);
        log.info("机器人-读取M11信号");
    }

    /**
     * 机器人-读取M20信号
     */
    @Override
    public void getRobotM20() {
        cmdHandleService.robotM20(168);
        log.info("机器人-读取M20信号");
    }

    /**
     * 机器人-读取M21信号
     */
    @Override
    public void getRobotM21() {
        cmdHandleService.robotM21(169);
        log.info("机器人-读取M21信号");
    }

    /**
     * 机器人-读取M22信号
     */
    @Override
    public void getRobotM22() {
        cmdHandleService.robotM22(170);
        log.info("机器人-读取M22信号");
    }

    /**
     * 机器人-读取M120信号
     */
    @Override
    public void getRobotM120() {
        cmdHandleService.robotM120(200);
        log.info("机器人-读取M120信号");
    }

    /**
     * 机器人-读取M121信号
     */
    @Override
    public void getRobotM121() {
        cmdHandleService.robotM121(201);
        log.info("机器人-读取M121信号");
    }

    /**
     * 机器人-读取M信号
     */
    @Override
    public void getRobotM(int address) {
        cmdHandleService.robotStatusM(address);
        log.info("机器人-读取M信号");
    }

    /**
     * 机器人-读取M122信号
     */
    @Override
    public void getRobotM122() {
        cmdHandleService.robotM122(202);
        log.info("机器人-读取M122信号");
    }

    /**
     * 机器人-读取M123信号
     */
    @Override
    public void getRobotM123() {
        cmdHandleService.robotM123(203);
        log.info("机器人-读取M123信号");
    }

    /**
     * 机器人-读取M124信号
     */
    @Override
    public void getRobotM124() {
        cmdHandleService.robotM124(204);
        log.info("机器人-读取M124信号");
    }

    /**
     * 机器人-机械臂 移动到药位
     */
    @Override
    public void robotRunToDrug(int[] imgPosition, int[] imgUpPosition) {

        int[] ints1 = new int[3];
        int[] ints2 = new int[3];

        for (int i = 0; i < imgPosition.length; i++) {
            ints1[i] = imgPosition[i];
        }
        for (int i = 0; i < imgUpPosition.length; i++) {
            ints2[i] = imgPosition[i];
        }

        //设置抓料位置 设置抓料上方位置
        cmdHandleService.pickLocation(ints1);
        cmdHandleService.pickUpLocation(ints2);

        //等待抓料信号 M10
        cmdHandleService.robotM10(160, true);

        log.info("机器人-机械臂 移动到药位");
    }

    @Override
    public void robotSetPickDrug(int[] realityPosition) {
        int[] ints1 = new int[3];

        for (int i = 0; i < realityPosition.length; i++) {
            ints1[i] = realityPosition[i];
        }

        ints1[2] = pickZ;

        cmdHandleService.pickLocation(ints1);
        DelayUtil.delayedExecution(1000, () -> {
            //等待抓料信号 M14
            cmdHandleService.robotM10(164, true);
        });
        log.info("设置实际抓料点完成");
    }

    /**
     * 机器人-机械臂 机械爪 抓取料袋
     */
    @Override
    public void robotWriteArmPick() {
        //抓料
        cmdHandleService.ioWriteCoil(1, "FF00");
        log.info("机器人-机械臂 抓取料袋");
        DelayUtil.delayedExecution(1000, new Runnable() {
            @Override
            public void run() {
                cmdHandleService.robotM111(193, true);
            }
        });


    }

    /**
     * 机器人-机械臂 机械爪抖动
     */
    @Override
    public void robotArmShake() {
        //到时候由气缸控制
        cmdHandleService.ioWriteCoil(5, "FF00");
        DelayUtil.delayedExecution(1000, () -> {
            cmdHandleService.ioWriteCoil(5, "0000");
            DelayUtil.delayedExecution(1000, () -> {
                cmdHandleService.ioWriteCoil(5, "FF00");
                DelayUtil.delayedExecution(1000, () -> {
                    cmdHandleService.ioWriteCoil(5, "0000");
                    DelayUtil.delayedExecution(1000, () -> {
                        cmdHandleService.ioWriteCoil(5, "FF00");
                        DelayUtil.delayedExecution(1000, () -> {
                            cmdHandleService.ioWriteCoil(5, "0000");
                            DelayUtil.delayedExecution(1000, () -> {
                                cmdHandleService.ioWriteCoil(5, "FF00");
                                DelayUtil.delayedExecution(1000, () -> {
                                    cmdHandleService.ioWriteCoil(5, "0000");
                                    DelayUtil.delayedExecution(1000, () -> {
                                        cmdHandleService.ioWriteCoil(5, "FF00");
                                        DelayUtil.delayedExecution(1000, () -> {
                                            cmdHandleService.ioWriteCoil(5, "0000");
                                            DelayUtil.delayedExecution(1000, () -> {
                                                cmdHandleService.ioWriteCoil(5, "FF00");
                                                DelayUtil.delayedExecution(1000, () -> {
                                                    cmdHandleService.ioWriteCoil(5, "0000");
                                                    DelayUtil.delayedExecution(1000, () -> {
                                                        cmdHandleService.ioWriteCoil(5, "FF00");
                                                        DelayUtil.delayedExecution(1000, () -> {
                                                            cmdHandleService.ioWriteCoil(5, "0000");
                                                            DelayUtil.delayedExecution(1000, () -> {
                                                                //写入M114抖料完成信号
                                                                cmdHandleService.robotM114(196, true);
                                                                log.info("测试-写入M114完成信号完成");
                                                            });
                                                        });
                                                    });
                                                });
                                            });
                                        });
                                    });
                                });
                            });
                        });
                    });
                });
            });
        });

        log.info("机器人-机械臂 机械爪抖动");
    }

    /**
     * 机器人-机械臂 放下袋子
     */
    @Override
    public void robotArmDownBag() {
        //夹具-松开动作
        cmdHandleService.ioWriteCoil(1, "0000");
        DelayUtil.delayedExecution(1000, new Runnable() {
            @Override
            public void run() {
                cmdHandleService.robotM110(192, true);
            }
        });
        log.info("机器人-机械臂 放下袋子");
    }

    /**
     * 机器人-机械臂 气缸下推
     */
    @Override
    public void robotArmAirDown() {
        //压板-气缸下推
        cmdHandleService.ioWriteCoil(3, "FF00");
        DelayUtil.delayedExecution(1000, new Runnable() {
            @Override
            public void run() {
                //写入M112抖料完成信号
                cmdHandleService.robotM112(194, true);
            }
        });
        log.info("机器人-机械臂 气缸下推");
    }

    /**
     * 机器人-机械臂 气缸上回
     */
    @Override
    public void robotArmAirUp() {
        //压板-气缸上回
        cmdHandleService.ioWriteCoil(3, "0000");
        DelayUtil.delayedExecution(1000, new Runnable() {
            @Override
            public void run() {
                //写入M113上回完成信号
                cmdHandleService.robotM113(195, true);
            }
        });

        log.info("机器人-机械臂 气缸上回");
    }

    /**
     * 机器人-启动动作
     */
    @Override
    public void robotPause(boolean open) {
        cmdHandleService.robotSetPause(open);
        log.info("机器人暂停动作");
    }


}
